package Dijkstra;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;

import roborally.Board;
import roborally.Position;

/**
 * @author Jef Keijers/Bachelor of Science: Informatics
 * @version 1.0
 */
public class Dijkstra {
	
	public DijkstraPath findShortestPath(Position initialPosition,Position goal,Board board) {
		DijkstraPath initialPath=new DijkstraPath(createInitialPath(initialPosition));
		PriorityQueue<DijkstraPath> queue = new PriorityQueue<DijkstraPath>();
		HashSet<DijkstraPath> alreadyVisited = new HashSet<DijkstraPath>();
		queue.add(initialPath);
		
		while (true) {
			DijkstraPath path=queue.poll();
			if (path.hasReached(goal)) {
				return path;
			} else if (!(alreadyVisited.contains(path))) {
				alreadyVisited.add(path);
				queue.addAll(path.extendPath(board));
			}
		}
	}
	
	private List <Position> createInitialPath(Position initialPosition) {
		ArrayList <Position> initialPath=new ArrayList<Position>();
		initialPath.add(initialPosition);
		return initialPath;
	}
}
